/*
 * @Author: LVGRAPE
 * @Date: 2023-05-18 11:19:07
 * @LastEditTime: 2023-12-03 15:52:59
 * @LastEditors: LVGRAPE
 * @Description:
 * @FilePath: \zino-fc-v4\ZINO\filter\filter.c
 * 可以输入预定的版权声明、个性签名、空行等
 */
#include <rtthread.h>
#include "filter.h"
#include "maths.h"
#include "math.h"
#include <string.h>
#define M_PI_F (float)3.14159265
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT  3.14159265358979323846f
#define BIQUAD_Q 1.0f / sqrtf(2.0f)  
void KalmanFilterInit(Kalman_t* kfp)
{
	kfp->Last_P = 1;
	kfp->Now_P = 0;
	kfp->out = 0;
	kfp->Kg = 0;
	kfp->Q = 0;
	kfp->R = 0.1;
}
/**
 * @brief
 *
 * @param kfp
 * @param q 过程噪声
 * @param r 测量噪声
 */
void KalmanFilterInitParam(Kalman_t* kfp, float q, float r)
{
	kfp->Last_P = 1;
	kfp->Now_P = 0;
	kfp->out = 0;
	kfp->Kg = 0;
	kfp->Q = q;
	kfp->R = r;
}

/**
 *卡尔曼滤波器
 *@param 	Kalman *kfp 卡尔曼结构体参数
 *   			float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
float KalmanFilter(Kalman_t* kfp, float input)
{
	// 预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
	kfp->Now_P = kfp->Last_P + kfp->Q;
	// 卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
	kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
	// 更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
	kfp->out = kfp->out + kfp->Kg * (input - kfp->out); // 因为这一次的预测值就是上一次的输出值
	// 更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
	kfp->Last_P = (1 - kfp->Kg) * kfp->Now_P;
	return kfp->out;
}

/**
 * 设置二阶低通滤波截至频率
 */
void lpf2pSetCutoffFreq(lpf2pData* lpfData, float sample_freq, float cutoff_freq)
{
	float fr = sample_freq / cutoff_freq;
	float ohm = tan_approx(M_PI_F / fr);
	float c = 1.0f + 2.0f * cos_approx(M_PI_F / 4.0f) * ohm + ohm * ohm;
	lpfData->b0 = ohm * ohm / c;
	lpfData->b1 = 2.0f * lpfData->b0;
	lpfData->b2 = lpfData->b0;
	lpfData->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
	lpfData->a2 = (1.0f - 2.0f * cos_approx(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
	lpfData->delay_element_1 = 0.0f;
	lpfData->delay_element_2 = 0.0f;
}

/**
 * 二阶低通滤波
 */
void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq)
{
	if (lpfData == NULL || cutoff_freq <= 0.0f)
	{
		return;
	}

	lpf2pSetCutoffFreq(lpfData, sample_freq, cutoff_freq);
}

float lpf2pApply(lpf2pData* lpfData, float sample)
{
	float delay_element_0 = sample - lpfData->delay_element_1 * lpfData->a1 - lpfData->delay_element_2 * lpfData->a2;
	if (!isfinite(delay_element_0))
	{
		// don't allow bad values to propigate via the filter
		delay_element_0 = sample;
	}

	float output = delay_element_0 * lpfData->b0 + lpfData->delay_element_1 * lpfData->b1 + lpfData->delay_element_2 * lpfData->b2;

	lpfData->delay_element_2 = lpfData->delay_element_1;
	lpfData->delay_element_1 = delay_element_0;
	return output;
}

float lpf2pReset(lpf2pData* lpfData, float sample)
{
	float dval = sample / (lpfData->b0 + lpfData->b1 + lpfData->b2);
	lpfData->delay_element_1 = dval;
	lpfData->delay_element_2 = dval;
	return lpf2pApply(lpfData, sample);
}
void slewFilterInit(slewFilter_t* filter, float slewLimit, float threshold)
{
	filter->state = 0.0f;
	filter->slewLimit = slewLimit;
	filter->threshold = threshold;
}

float slewFilterApply(slewFilter_t* filter, float input)
{
	if (filter->state >= filter->threshold) {
		if (input >= filter->state - filter->slewLimit) {
			filter->state = input;
		}
	}
	else if (filter->state <= -filter->threshold) {
		if (input <= filter->state + filter->slewLimit) {
			filter->state = input;
		}
	}
	else {
		filter->state = input;
	}
	return filter->state;
}

// get notch filter Q given center frequency (f0) and lower cutoff frequency (f1)
// Q = f0 / (f2 - f1) ; f2 = f0^2 / f1
float filterGetNotchQ(float centerFreq, float cutoffFreq) {
	return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq);
}

/* sets up a biquad Filter */
/**
 * @brief 
 * 
 * @param filter 
 * @param filterFreq cutoff freq (Hz)
 * @param refreshRate refresh time (us)
 */
void biquadFilterInitLPF(biquadFilter_t* filter, float filterFreq, uint32_t refreshRate)
{
	biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}
/**
 * @brief 
 * 
 * @param filter 
 * @param filterFreq Hz
 * @param refreshRate time in us
 * @param Q 
 * @param filterType 
 */
void biquadFilterInit(biquadFilter_t* filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
	// setup variables
	const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f;
	const float sn = sin_approx(omega);
	const float cs = cos_approx(omega);
	const float alpha = sn / (2.0f * Q);

	float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0;

	switch (filterType) {
	case FILTER_LPF:
		// 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
		// described in http://www.ti.com/lit/an/slaa447/slaa447.pdf
		b0 = (1 - cs) * 0.5f;
		b1 = 1 - cs;
		b2 = (1 - cs) * 0.5f;
		a0 = 1 + alpha;
		a1 = -2 * cs;
		a2 = 1 - alpha;
		break;
	case FILTER_NOTCH:
		b0 = 1;
		b1 = -2 * cs;
		b2 = 1;
		a0 = 1 + alpha;
		a1 = -2 * cs;
		a2 = 1 - alpha;
		break;
	case FILTER_BPF:
		b0 = alpha;
		b1 = 0;
		b2 = -alpha;
		a0 = 1 + alpha;
		a1 = -2 * cs;
		a2 = 1 - alpha;
		break;
	}

	// precompute the coefficients
	filter->b0 = b0 / a0;
	filter->b1 = b1 / a0;
	filter->b2 = b2 / a0;
	filter->a1 = a1 / a0;
	filter->a2 = a2 / a0;

	// zero initial samples
	filter->x1 = filter->x2 = 0;
	filter->y1 = filter->y2 = 0;
}

void biquadFilterUpdate(biquadFilter_t* filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
	// backup state
	float x1 = filter->x1;
	float x2 = filter->x2;
	float y1 = filter->y1;
	float y2 = filter->y2;

	biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType);

	// restore state
	filter->x1 = x1;
	filter->x2 = x2;
	filter->y1 = y1;
	filter->y2 = y2;
}

void biquadFilterUpdateLPF(biquadFilter_t* filter, float filterFreq, uint32_t refreshRate)
{
	biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}

/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
float biquadFilterApplyDF1(biquadFilter_t* filter, float input)
{
	/* compute result */
	const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;

	/* shift x1 to x2, input to x1 */
	filter->x2 = filter->x1;
	filter->x1 = input;

	/* shift y1 to y2, result to y1 */
	filter->y2 = filter->y1;
	filter->y1 = result;

	return result;
}

/* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
float biquadFilterApply(biquadFilter_t* filter, float input)
{
	const float result = filter->b0 * input + filter->x1;
	filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
	filter->x2 = filter->b2 * input - filter->a2 * result;
	return result;
}

void laggedMovingAverageInit(laggedMovingAverage_t* filter, uint16_t windowSize, float* buf)
{
	filter->movingWindowIndex = 0;
	filter->windowSize = windowSize;
	filter->buf = buf;
	filter->movingSum = 0;
	memset(filter->buf, 0, windowSize * sizeof(float));
	filter->primed = false;
}

float laggedMovingAverageUpdate(laggedMovingAverage_t* filter, float input)
{
	filter->movingSum -= filter->buf[filter->movingWindowIndex];
	filter->buf[filter->movingWindowIndex] = input;
	filter->movingSum += input;

	if (++filter->movingWindowIndex == filter->windowSize) {
		filter->movingWindowIndex = 0;
		filter->primed = true;
	}

	const uint16_t denom = filter->primed ? filter->windowSize : filter->movingWindowIndex;
	return filter->movingSum / denom;
}
